#ifndef __OCPP_Cesium_scaleToGeodeticSurface_H__
#define __OCPP_Cesium_scaleToGeodeticSurface_H__

#include "CesiumMath.h"
#include "Cartesian3.h"

namespace OCPP
{
	namespace Cesium
	{
		/**
		 * Scales the provided Cartesian position along the geodetic surface normal
		 * so that it is on the surface of this ellipsoid.  If the position is
		 * at the center of the ellipsoid, this function returns undefined.
		 *
		 * @param {Cartesian3} cartesian The Cartesian position to scale.
		 * @param {Cartesian3} oneOverRadii One over radii of the ellipsoid.
		 * @param {Cartesian3} oneOverRadiiSquared One over radii squared of the ellipsoid.
		 * @param {number} centerToleranceSquared Tolerance for closeness to the center.
		 * @param {Cartesian3} [result] The object onto which to store the result.
		 * @returns {Cartesian3} The modified result parameter, a new Cartesian3 instance if none was provided, or undefined if the position is at the center.
		 *
		 * @function scaleToGeodeticSurface
		 *
		 * @private
		 */
		inline Cartesian3 scaleToGeodeticSurface1(
			Cartesian3 cartesian,
			Cartesian3 oneOverRadii,
			Cartesian3 oneOverRadiiSquared,
			double centerToleranceSquared
		) {
			auto positionX = cartesian.x;
			auto positionY = cartesian.y;
			auto positionZ = cartesian.z;

			auto oneOverRadiiX = oneOverRadii.x;
			auto oneOverRadiiY = oneOverRadii.y;
			auto oneOverRadiiZ = oneOverRadii.z;

			auto x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX;
			auto y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY;
			auto z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ;

			// Compute the squared ellipsoid norm.
			auto squaredNorm = x2 + y2 + z2;
			auto ratio = sqrt(1.0 / squaredNorm);

			// As an initial approximation, assume that the radial intersection is the projection point.
			auto intersection = Cartesian3::multiplyByScalar(
				cartesian,
				ratio
			);

			// If the position is near the center, the iteration will not converge.
			if (squaredNorm < centerToleranceSquared) {
				return !std::isfinite(ratio)
					? Cartesian3::ZERO
					: intersection;
			}

			auto oneOverRadiiSquaredX = oneOverRadiiSquared.x;
			auto oneOverRadiiSquaredY = oneOverRadiiSquared.y;
			auto oneOverRadiiSquaredZ = oneOverRadiiSquared.z;

			// Use the gradient at the intersection point in place of the true unit normal.
			// The difference in magnitude will be absorbed in the multiplier.
			auto gradient = Cartesian3();
			gradient.x = intersection.x * oneOverRadiiSquaredX * 2.0;
			gradient.y = intersection.y * oneOverRadiiSquaredY * 2.0;
			gradient.z = intersection.z * oneOverRadiiSquaredZ * 2.0;

			// Compute the initial guess at the normal vector multiplier, lambda.
			auto lambda =
				((1.0 - ratio) * Cartesian3::magnitude(cartesian)) /
				(0.5 * Cartesian3::magnitude(gradient));
			auto correction = 0.0;

			double func;
			double denominator;
			double xMultiplier;
			double yMultiplier;
			double zMultiplier;
			double xMultiplier2;
			double yMultiplier2;
			double zMultiplier2;
			double xMultiplier3;
			double yMultiplier3;
			double zMultiplier3;

			do {
				lambda -= correction;

				xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX);
				yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY);
				zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ);

				xMultiplier2 = xMultiplier * xMultiplier;
				yMultiplier2 = yMultiplier * yMultiplier;
				zMultiplier2 = zMultiplier * zMultiplier;

				xMultiplier3 = xMultiplier2 * xMultiplier;
				yMultiplier3 = yMultiplier2 * yMultiplier;
				zMultiplier3 = zMultiplier2 * zMultiplier;

				func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0;

				// "denominator" here refers to the use of this expression in the velocity and acceleration
				// computations in the sections to follow.
				denominator =
					x2 * xMultiplier3 * oneOverRadiiSquaredX +
					y2 * yMultiplier3 * oneOverRadiiSquaredY +
					z2 * zMultiplier3 * oneOverRadiiSquaredZ;

				auto derivative = -2.0 * denominator;

				correction = func / derivative;
			} while (abs(func) > CesiumMath::EPSILON12);

			Cartesian3 result;
			result.x = positionX * xMultiplier;
			result.y = positionY * yMultiplier;
			result.z = positionZ * zMultiplier;
			return result;
		}
	}
}

#endif

